/* 
 * File:   multisurfalign_grid3d.cc
 * Author: boyan
 * 
 * Created on April 17, 2009, 12:59 PM
 */

#include "grid3d.h"

#include <map>
#include <set>
#include <cmath>
#include <vector>
#include <bitset>
#include <sstream>
#include <stddef.h>
#include <assert.h>
#include <algorithm>

#include "errlog.h"
#include "SurfAlign_grid3d.h"
#include "MultiSurfAlign_grid3d.h"
#include "cxx_misc.h"

#include <Matrix3D.hpp>
#include <MsqError.hpp>

MultiSurfAlign_grid3d::MultiSurfAlign_grid3d(grid3d_t *g)
: SurfAlign_grid3d(g)
{
    MakeInverse(g);
    clear_patches();
    check_codes();
}

MultiSurfAlign_grid3d::MultiSurfAlign_grid3d(grid3d_t *g, std::vector<LevelsetCode> &ls)
: SurfAlign_grid3d(g, ls)
{
    MakeInverse(g);
    clear_patches();
    check_codes();
}

void MultiSurfAlign_grid3d::domain_DoF(const Mesh::EntityHandle* handle_array,
                                       unsigned short* dof_array,
                                       size_t num_handles, Mesquite::MsqError& err) const
{
    for (size_t hid = 0; hid < num_handles; ++hid)
    {
        Mesh::EntityHandle h = handle_array[hid];
        unsigned short &dof = dof_array[hid];
        if (curve_of_handle.find(h) != curve_of_handle.end())
            dof = 1;
        else if (patch_of_handle.find(h) != patch_of_handle.end())
            dof = 2;
        else
            dof = 3;
    }
}

void MultiSurfAlign_grid3d::get_codes(const Mesh::EntityHandle *handles,
                                      unsigned codes[], size_t count)
{
    for (size_t hid = 0; hid < count; ++hid)
    {
        Mesh::EntityHandle h = handles[hid];
        unsigned &code = codes[hid];
        std::map<Mesh::VertexHandle, int>::iterator found;
        if ((found = curve_of_handle.find(h)) != curve_of_handle.end())
        {
            if (found->second < 0)
                error_msg("%s: invalid curve for handle\n", __func__);
            code = curves[found->second].code;
        }
        else if ((found = patch_of_handle.find(h)) != patch_of_handle.end())
        {
            if (found->second < 0)
                error_msg("%s: invalid patch for handle\n", __func__);
            code = patches[found->second].code;
        }
        else
            code = 0;
    }
}

void MultiSurfAlign_grid3d::set_codes(const Mesh::EntityHandle *handles,
                                      const unsigned codes[], size_t count)
{
    for (size_t idx = 0; idx < count; ++idx)
        if (codes[idx])
        {
            size_t jdx, num_patches = 0;
            int last_patch = -1;
            unsigned ccode = 0;
            for (jdx = 0; jdx < patches.size(); ++jdx)
                if (patches[jdx].code & codes[idx])
                {
                    ccode |= patches[jdx].code;
                    ++num_patches;
                    last_patch = jdx;
                }
            switch (num_patches)
            {
            case 0:
                break;
            case 1:
                patch_of_handle[handles[idx]] = last_patch;
                curve_of_handle.erase(handles[idx]);
                break;
            case 2:
                for (jdx = 0; jdx < curves.size(); ++jdx)
                    if (curves[jdx].code == ccode)
                    {
                        curve_of_handle[handles[idx]] = jdx;
                        patch_of_handle.erase(handles[idx]);
                        break;
                    }
                assert(jdx < curves.size());
                break;
            default:
                error_msg("%s: geometry with vertices is not implemented.\n",
                          __func__);
            }
        }
}

void MultiSurfAlign_grid3d::clear_handles(void)
{
    curve_of_handle.clear();
    patch_of_handle.clear();
}

void MultiSurfAlign_grid3d::snap_point_to_curve(Mesh::EntityHandle handle,
                                                const Vector3D &position)
{
    (void) position;
    curve_of_handle[handle] = -1;
    patch_of_handle.erase(handle);
}

void MultiSurfAlign_grid3d::snap_point_to_surface(Mesh::EntityHandle handle,
                                                  const Vector3D &position)
{
    (void) position;
    if (curve_of_handle.find(handle) == curve_of_handle.end())
        patch_of_handle[handle] = -1;
}

void MultiSurfAlign_grid3d::check_codes() const
{
    for (size_t idx = 0; idx < surf_nodes.size(); ++idx)
        if (grid->nodes[surf_nodes[idx].id].on_bdry == 0)
            error_msg("node %d is not on boundary\n", surf_nodes[idx].id);

}

void MultiSurfAlign_grid3d::add_patch(unsigned code, const char* name)
{
    SurfacePatch *sc = NULL;
    for (size_t idx = 0; sc == NULL && idx < patches.size(); ++idx)
        if (patches[idx].is_name(name))
            sc = &patches[idx];
    if (sc != NULL)
        sc->code |= code;
    else
        patches.push_back(SurfacePatch(this, code, name));
}

void MultiSurfAlign_grid3d::check_patches_overlap() const
{
    std::bitset < 8 * sizeof (unsigned) > acc_codes(0);
    for (size_t s = 0; s < patches.size(); ++s)
    {
        if ((acc_codes.to_ulong() & patches[s].code) != 0)
            error_msg("%s: surface patches overlap\n", __func__);
        acc_codes |= patches[s].code;
    }
    //    size_t max_code_bit=((int)log2(acc_codes.to_ulong()))+1;
    //    for(size_t ii=0; ii<max_code_bit; ii++)
    //        if(acc_codes.test(ii)==false)
    //            error_msg("%s: patches do not cover the whole surface\n",
    //                      __func__);
}

std::string MultiSurfAlign_grid3d::patches_to_string() const
{
    std::ostringstream ostr;
    std::vector<SurfacePatch>::const_iterator it;
    for (it = patches.begin(); it != patches.end(); ++it)
        ostr << it->to_string() << std::endl;
    return ostr.str();
}

void MultiSurfAlign_grid3d::compile_patches()
{
    std::vector<SurfacePatch>::iterator S;
    std::vector<SurfFace>::iterator F;

    // make sure surface patches  do not overlap
    check_patches_overlap();

    // fill in the patches_of_xxxxx arrays and count faces of each patch
    std::vector<size_t> NF;
    NF.resize(patches.size(), 0);

    patch_id_of_face.resize(surf_faces.size(), 0);
    patch_ids_of_node.resize(surf_nodes.size());
    patch_ids_of_edge.resize(surf_edges.size());
    for (F = surf_faces.begin(); F != surf_faces.end(); ++F)
    {
        size_t face_id = F - surf_faces.begin();
        // find the patch of the face
        unsigned common_code;
        common_code = grid->nodes[F->nodes(0)].on_bdry;
        common_code &= grid->nodes[F->nodes(1)].on_bdry;
        common_code &= grid->nodes[F->nodes(2)].on_bdry;
        size_t patch_id = (size_t) - 1;
        for (S = patches.begin(); S < patches.end(); ++S)
            if (S->has_code(common_code))
            {
                if (patch_id != (size_t) - 1)
                    error_msg("%s: face %d belongs to more than one patch!?\n",
                              __func__, F->id);
                patch_id = S - patches.begin();
            }
        if (patch_id == (size_t) - 1)
            error_msg("%s: face %ld does not belong to any patch\n",
                      __func__, face_id);
        // face, its edges and nodes belong to patch patch
        patch_id_of_face[face_id] = patch_id;
        for (int i = 0; i < 3; i++)
            patch_ids_of_node[F->surf_nodes(i, nodes_inv)].insert(patch_id);
        for (int i = 0; i < 3; i++)
            patch_ids_of_edge[F->surf_edges(i, edges_inv)].insert(patch_id);
        // count face for its patch
        ++NF[patch_id];
    }

    // fill in the sf_ids array of each patch
    for (S = patches.begin(); S < patches.end(); ++S)
    {
        SurfacePatch &P = S.operator *();
        size_t patch_id = S - patches.begin();
        P.sf_ids.reserve(NF[patch_id]);
    }
    for (F = surf_faces.begin(); F < surf_faces.end(); ++F)
    {
        size_t face_id = F - surf_faces.begin();
        size_t patch_id = patch_id_of_face[face_id];
        patches[patch_id].sf_ids.push_back(face_id);
    }

    //fill in the se_ids array of each patch
    std::vector<size_t> NE(patches.size()); // number of edges in each patch;
    std::vector<SurfEdge>::iterator E;
    for (E = surf_edges.begin(); E != surf_edges.end(); ++E)
    {
        size_t se_id = E - surf_edges.begin();
        std::set<size_t> &p_ids = patch_ids_of_edge[se_id];
        std::set<size_t>::iterator p;
        for (p = p_ids.begin(); p != p_ids.end(); ++p)
            ++NE[p.operator *()];
    }
    for (S = patches.begin(); S < patches.end(); ++S)
    {
        SurfacePatch &P = S.operator *();
        size_t patch_id = S - patches.begin();
        P.se_ids.reserve(NE[patch_id]);
    }
    for (E = surf_edges.begin(); E != surf_edges.end(); ++E)
    {
        size_t se_id = E - surf_edges.begin();
        std::set<size_t> &p_ids = patch_ids_of_edge[se_id];
        std::set<size_t>::iterator p;
        for (p = p_ids.begin(); p != p_ids.end(); ++p)
            patches[*p].se_ids.push_back(se_id);
    }

    //fill in the sn_ids array of each patch
    std::vector<size_t> NN(patches.size()); // number of nodes in each patch;
    std::vector<SurfNode>::iterator Q;
    for (Q = surf_nodes.begin(); Q != surf_nodes.end(); ++Q)
    {
        size_t sn_id = Q - surf_nodes.begin();
        std::set<size_t> &p_ids = patch_ids_of_node[sn_id];
        std::set<size_t>::iterator p;
        for (p = p_ids.begin(); p != p_ids.end(); ++p)
            ++NN[p.operator *()];
    }
    for (S = patches.begin(); S < patches.end(); ++S)
    {
        SurfacePatch &P = S.operator *();
        size_t patch_id = S - patches.begin();
        P.sn_ids.reserve(NN[patch_id]);
    }
    for (Q = surf_nodes.begin(); Q != surf_nodes.end(); ++Q)
    {
        size_t sn_id = Q - surf_nodes.begin();
        std::set<size_t> &p_ids = patch_ids_of_node[sn_id];
        std::set<size_t>::iterator p;
        for (p = p_ids.begin(); p != p_ids.end(); ++p)
            patches[*p].sn_ids.push_back(sn_id);
    }

    // make vertices -- these are nodes that belong to more than 2 patches
    compile_vertices();

    // make adjacent patches;
    std::vector<std::set<size_t> >::iterator PI;
    for (PI = patch_ids_of_edge.begin(); PI != patch_ids_of_edge.end(); ++PI)
        if (PI->size() == 2)
        {
            size_t e_patches[2] = {*PI->begin(), *(++PI->begin())};
            patches[e_patches[0]].adj_patch_ids.insert(e_patches[1]);
            patches[e_patches[1]].adj_patch_ids.insert(e_patches[0]);
        }

    // this is an unnecessary check.  patch adjacent to only one other patch
    // has no vertices
    for (S = patches.begin(); S != patches.end(); ++S)
        if (S->adj_patch_ids.size() == 1 && S->vertex_ids.size() > 0)
            error_msg("%s: patch %u is adjacent to only one other patch (%u) "
                      "and has %u vertices.\n", __func__,
                      (unsigned) (S - patches.begin()),
                      (unsigned) *(S->adj_patch_ids.begin()),
                      (unsigned) S->vertex_ids.size());

    // make closed curves (around patches with only one adjacent patch)
    compile_closed_curves();
}

void MultiSurfAlign_grid3d::push_closed_curve(size_t patch_id)
{
    SurfacePatch &patch = patches[patch_id];
    size_t adj_patch_id = *patch.adj_patch_ids.begin();
    SurfFace F = surf_faces.front();
    SurfaceCurve curve(this);
    curve.closed = true;
    curve.patch_ids[0] = patch_id;
    curve.patch_ids[1] = adj_patch_id;
    curve.code = patch.code | patches[adj_patch_id].code;
    sort2(curve.patch_ids);
    curve.vertex_ids[0] = curve.vertex_ids[1] = (size_t) - 1;

    //  see if this curve already exists
    if (find_curve(curve) != (size_t) - 1)
        return;

    // now get edges;
    size_t NE = get_edges_of_patch(patch, curve.se_ids);

    // now sort edges and get nodes (in order going counterclockwise around patch
    curve.sn_ids.reserve(NE);
    // find face containing the first edge
    size_t first_face_id = (size_t) - 1;
    size_t first_face_edge_id;
    size_t first_edge_id = curve.se_ids.front();
    bool done = false;
    std::vector<size_t>::iterator it;
    for (it = patch.sf_ids.begin(); it != patch.sf_ids.end(); ++it)
    {
        size_t face_id = it.operator *();
        size_t idx;
        F = surf_faces[face_id];
        for (idx = 0; idx < 3; ++idx)
            if (F.surf_edges(idx, edges_inv) == (int) first_edge_id)
            {
                first_face_id = face_id;
                first_face_edge_id = idx;
                done = true;
                break;
            }
        if (done)
            break;
    }
    assert(first_face_id != (size_t) - 1);
    size_t first_face_node_id = (first_face_edge_id + (F.swap_normal ? 2 : 1)) % 3;
    size_t first_node_id = F.surf_nodes(first_face_node_id, nodes_inv);
    curve.sn_ids.push_back(first_node_id);

    // now add nodes and edges in order going counterclockwise about patch
    while (curve.sn_ids.size() < NE)
    {
        // current position
        size_t current = curve.sn_ids.size() - 1;
        // current edge // current node is one of the nodes of E
        SurfEdge &E = surf_edges[curve.se_ids[current]];
        // next node is the other node of current edge E
        size_t next_node = E.other_surf_node(curve.sn_ids.back(), nodes_inv);
        // find the other edge of next_node
        size_t next_edge_pos;
        // we only check the remaining edges, so we avoid current edge E
        for (next_edge_pos = current + 1; next_edge_pos < NE; ++next_edge_pos)
            if (surf_edges[curve.se_ids[next_edge_pos]].has_surf_node(next_node, nodes_inv))
                break;
        assert(next_edge_pos < NE);
        // swap the next edge and the edge we want to be next
        std::swap(curve.se_ids.at(current + 1), curve.se_ids.at(next_edge_pos));
        // add next node to sn_ids
        curve.sn_ids.push_back(next_node);
    }

#ifdef NDEBUG
    return;
#else
    // unnecessary check: last edge in se_ids has the last node in sn_ids as
    // one of its nodes; the other node must be the first node in sn_ids.
    SurfEdge &E = surf_edges[curve.se_ids.back()];
    assert(E.has_surf_node(curve.sn_ids.front(), nodes_inv));
#endif

    // add curve to curves
    curves.push_back(curve);
    size_t curve_id = curves.size() - 1;
    patches[curve.patch_ids[0]].curve_ids.push_back(curve_id);
    patches[curve.patch_ids[1]].curve_ids.push_back(curve_id);

}

void MultiSurfAlign_grid3d::compile_vertices()
{
    vertices.clear();
    std::vector<std::set<size_t> >::iterator PI;
    for (PI = patch_ids_of_node.begin(); PI != patch_ids_of_node.end(); ++PI)
        if (PI->size() > 2)
        {
            error_msg("%s: topologies with vertices are "
                      "not yet implemented.\n", __func__);
            // TODO:  implement topologies with vertices
            size_t sn_id = PI - patch_ids_of_node.begin();
            vertices.push_back(SurfaceVertex(sn_id));
            vertices.back().curve_ids.clear();
            vertices.back().patch_ids = (PI.operator *());
            std::set<size_t>::iterator it;
            for (it = PI->begin(); it != PI->end(); ++it)
                patches[*it].vertex_ids.insert(sn_id);
        }
}

void MultiSurfAlign_grid3d::compile_closed_curves()
{
    std::vector<SurfacePatch>::iterator S;
    curves.clear();
    for (S = patches.begin(); S != patches.end(); ++S)
        if (S->adj_patch_ids.size() == 1)
            push_closed_curve(S - patches.begin());
}

size_t MultiSurfAlign_grid3d::find_curve(const SurfaceCurve& c) const
{
    std::vector<SurfaceCurve>::const_iterator C;
    for (C = curves.begin(); C != curves.end(); ++C)
        if (c == *C)
            return C - curves.begin();
    return (size_t) - 1;
}

size_t MultiSurfAlign_grid3d::get_edges_of_patch(const SurfacePatch &patch,
                                                 std::vector<size_t> &edge_ids)
{
    std::vector<size_t> foo;
    foo.resize(surf_edges.size());
    std::vector<size_t>::const_iterator it;
    for (it = patch.sf_ids.begin(); it != patch.sf_ids.end(); ++it)
    {
        size_t face_id = it.operator *();
        SurfFace &F = surf_faces[face_id];
        ++foo[F.surf_edges(0, edges_inv)];
        ++foo[F.surf_edges(1, edges_inv)];
        ++foo[F.surf_edges(2, edges_inv)];
    }
    size_t NE = 0;
    for (it = foo.begin(); it != foo.end(); it++)
        if (*it == 1)
            NE++;
    edge_ids.reserve(NE);
    for (it = foo.begin(); it != foo.end(); it++)
        if (*it == 1)
            edge_ids.push_back(it - foo.begin());
    return NE;
}

/****************************************************************************
 ****************************************************************************
 ****************************************************************************
 ****************************************************************************/

size_t MultiSurfAlign_grid3d::find_closest_node
(Mesh::EntityHandle handle, const Vector3D position) const
{
    // Find the nearest node
    size_t min_idx = (size_t) - 1;
    double dist, min_dist = 1e99; // large number
    for (size_t idx = 0; /*(grid->MAXH * 0.1 < min_dist) && */ (idx < surf_nodes.size()); idx++)
    {
        dist = dist_l2_2(position.to_array(), surf_nodes[idx].toArray());
        if (dist < min_dist)
        {
            min_idx = idx;
            min_dist = dist;
        }
    }
    return min_idx;
}

struct SurfFacePlusPlus : public SurfFace
{

    SurfFacePlusPlus(const SurfFace & rhs) : SurfFace(rhs)
    {
    }
    // returns the barycentric coordinates of the projection of pt 

    void GetBaryCoordsOfProj(const Vector3D &pt, double *bary)
    {
        const int *my_nodes = nodes();
        Vector3D n[3];
        for (int i = 0; i < 3; i++)
            n[i].set(grid->nodes[my_nodes[i]].coords);
        Vector3D N = // cross product; the order matters
                (n[1] - n[0]) * (n[2] - n[0]);
        Vector3D e0(pt - n[0]), e1(pt - n[1]), e2(pt - n[2]);
        bary[0] = Mesquite::det(Mesquite::Matrix3D(e2, e1, N));
        bary[1] = Mesquite::det(Mesquite::Matrix3D(e2, N, e0));
        bary[2] = Mesquite::det(Mesquite::Matrix3D(N, e1, e0));
        double p = bary[0] + bary[1] + bary[2];
        bary[0] /= p;
        bary[1] /= p;
        bary[2] /= p;
    }
};

void MultiSurfAlign_grid3d::closest_point
(Mesh::EntityHandle handle,
 const Vector3D& position, Vector3D& closest,
 Vector3D& normal, MsqError& err) const
{
    // to avoid compiler warning:
    (void) err;

    std::map<Mesh::VertexHandle, int>::iterator found;

    // does handle snap to a curve
    found = curve_of_handle.find(handle);
    if (found != curve_of_handle.end())
    {
        int curve_no = found->second;
        assert(curve_no < (int) curves.size());
        // is the curve of the handle known
        if (curve_no >= 0)
        {
            curves[curve_no].closest_point(handle, position, closest, normal, err);
        }
        else
        {
            // find which curve is closest to snap to
            double dist, min_dist = 1e99;
            Vector3D c, n;
            for (curve_no = 0; curve_no < (int) curves.size(); ++curve_no)
            {
                curves[curve_no].closest_point(handle, position, c, n, err);
                dist = dist_l2_2(position.to_array(), c.to_array());
                if (dist < min_dist)
                {
                    closest = c;
                    normal = n;
                    found->second = curve_no;
                    min_dist = dist;
                }
            }
        }
        return;
    }

    // does handle snap to a patch
    found = patch_of_handle.find(handle);
    bool snap_to_surface = found != patch_of_handle.end();
    int &patch_no = found->second;

    if (snap_to_surface && patch_no >= 0)
    {
        patches[patch_no].closest_point(handle, position, closest, normal, err);
        return;
    }

    // find the closest surface node
    size_t closest_node_idx = find_closest_node(handle, position);
    const std::set<size_t> &patches_of_closest = patch_ids_of_node[closest_node_idx];
    std::set<size_t>::iterator pc = patches_of_closest.begin();

    switch (patches_of_closest.size())
    {
    case 0: // this is impossible
        error_msg("%s: find_closest_node returned node "
                  "not on surface.\n", __func__);
    case 1: // node is inside patch
    {
        const SurfNode &N = surf_nodes[closest_node_idx];
        normal = N.GetSurfNormal(position);
        closest = N.GetProjection(position);
        if (snap_to_surface)
            patch_no = *pc;
        return;
    }
    case 2: // node belongs to a curve (snap to curve)
    {
        if (!snap_to_surface)
            project_to_curve(closest_node_idx, position, closest, normal);
        else
        {
            SurfNode N(surf_nodes[closest_node_idx]);
            int p0_val = *pc, p1_val = *(++pc);
            __compute_ball(N, &patches[p0_val].se_ids,
                           &patches[p0_val].sf_ids);
            Vector3D Proj0 = N.GetProjection(position);
            Vector3D Normal0 = N.GetSurfNormal(position);
            double dist0 = dist_l2_2(position.to_array(), Proj0.to_array());

            __compute_ball(N, &patches[p1_val].se_ids,
                           &patches[p1_val].sf_ids);
            Vector3D Proj1 = N.GetProjection(position);
            Vector3D Normal1 = N.GetSurfNormal(position);
            double dist1 = dist_l2_2(position.to_array(), Proj1.to_array());

            bool belongs[2] = {false, false};
            node_inv_t *ninv = grid->inverse->ninv + N.id;
            for (int i = 0; i < ninv->NF; i++)
            {
                int fid = ninv->fids[i];
                int sf_id = faces_inv[fid];
                if (sf_id < 0) // face is not on surface
                    continue;
                int current_patch = patch_id_of_face[sf_id];
                assert(current_patch == p0_val || current_patch == p1_val);
                SurfFacePlusPlus F = surf_faces[sf_id];
                double bary[3];
                F.GetBaryCoordsOfProj(position, &bary[0]);
                if (fmin(fmin(bary[0], bary[1]), bary[2]) > -1e-6)
                { // belongs
                    if (current_patch == p0_val)
                        belongs[0] = true;
                    else belongs[1] = true;
                }
            }
            int doo;

            if (belongs[0] == belongs[1])
                doo = (dist0 < dist1) ? 0 : 1;
            else
                doo = belongs[0] ? 0 : 1;

            switch (doo)
            {
            case 0:
                closest = Proj0;
                normal = Normal0;
                patch_no = p0_val;
                break;
            case 1:
                closest = Proj1;
                normal = Normal1;
                patch_no = p1_val;
                break;
            }
        }
        return;
    }
    default: // node is a vertex
        error_msg("%s: geometry with vertices is not implemented.\n", __func__);
    }
}

void MultiSurfAlign_grid3d::
project_to_curve(size_t sn_id, const Vector3D& position,
                 Vector3D& closest, Vector3D & normal) const
{
    // 0.a find curve
    std::set<size_t> p_ids = patch_ids_of_node[sn_id];
    size_t idx, pids[2] = {*(p_ids.begin()), *(++p_ids.begin())};
    sort2(pids);
    SurfaceCurve curve = curves[0];
    for (idx = 0; idx < curves.size(); idx++)
    {
        curve = curves[idx];
        if (curve.patch_ids[0] == pids[0] && curve.patch_ids[1] == pids[1])
            break;
    }
    assert(idx < curves.size());

    // 0.b find three nodes on circle
    Vector3D X0, X1, X2;
    size_t pos = curve.find_sn_id(sn_id);
    SurfNode node = surf_nodes[curve.sn_ids[pos]];
    X1 = node.toVector3D();
    if (pos > 0)
        X0 = surf_nodes[curve.sn_ids[pos - 1]].toVector3D();
    else
        X0 = surf_nodes[curve.sn_ids.back()].toVector3D();
    if (pos < curve.sn_ids.size() - 1)
        X2 = surf_nodes[curve.sn_ids[pos + 1]].toVector3D();
    else
        X2 = surf_nodes[curve.sn_ids.front()].toVector3D();

    // 1. find normal to plane through X0, X1, X2
    Vector3D N = (X2 - X1) * (X0 - X1); // operator *() of two Vector3D is cross product
    N.normalize();

    // 2. find R
    Mesquite::Matrix3D invM, Mat(X2 - X1, X0 - X1, N); // init by columns
    Vector3D Rhs(
                 0.5 * X2.length_squared() - 0.5 * X1.length_squared(),
                 0.5 * X0.length_squared() - 0.5 * X1.length_squared(),
                 N % X0); // operator %() of two Vector3D is inner product
    inv(invM, transpose(Mat));
    Vector3D R = invM * Rhs;

    // 3. find S -- projection of position on plane through X0, X1, X2
    double alpha = ((position - X1) % N);
    Vector3D S = position - alpha * N;

    // 4. find Q -- projection of position on circle through X0, X1, X2
    alpha = dist_l2(R.to_array(), X1.to_array()); // radius
    double beta = dist_l2(R.to_array(), S.to_array());
    alpha = alpha / beta;
    if (!std::isfinite(alpha))
        closest = X1;
    else
    {

        Vector3D Q = R + alpha * (S - R);
        Vector3D Q1 = R - alpha * (S - R);
        if (dist_l2_2(Q1.to_array(), X1.to_array())
                < dist_l2_2(Q.to_array(), X1.to_array()))
            Q = Q1;

        // 5.
        closest = Q;
    }
    normal = node.GetSurfNormal(closest);
}

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

LevelsetCode MultiSurfAlign_grid3d::SurfaceCurve::levelset_code
(Mesquite::Mesh::EntityHandle entity_handle,
 const Mesquite::Vector3D&coordinate) const
{
    unsigned handle_code, inside_patch_code;
    surf_domain->get_codes(&entity_handle, &handle_code, 1);
    if (handle_code == this->code)
        return ls_onsurf;
    inside_patch_code = surf_domain->patches[patch_ids[0]].code;
    if (surf_domain->patches[patch_ids[0]].adj_patch_ids.size() > 1)
        inside_patch_code = surf_domain->patches[patch_ids[1]].code;
    if (handle_code == inside_patch_code)
        return ls_inside;
    return ls_outside;
}

void MultiSurfAlign_grid3d::SurfaceCurve::snap_to
(Mesh::EntityHandle entity_handle, Vector3D &coordinate) const
{
    Mesquite::MsqError err;
    Vector3D pos = coordinate, normal;
    closest_point(entity_handle, pos, coordinate, normal, err);
}

void MultiSurfAlign_grid3d::SurfaceCurve::normal_at
(Mesh::EntityHandle entity_handle, Vector3D &coordinate) const
{
    Mesquite::MsqError err;
    Vector3D pos = coordinate, closest;
    closest_point(entity_handle, pos, closest, coordinate, err);
}

void MultiSurfAlign_grid3d::SurfaceCurve::normal_at
(const Mesh::EntityHandle* handles,
 Vector3D coordinates[],
 unsigned count,
 Mesquite::MsqError& err) const
{
    for (unsigned a = 0; a < count; a++)
        this->normal_at(handles[a], coordinates[a]);
}

void MultiSurfAlign_grid3d::SurfaceCurve::closest_point
(Mesh::EntityHandle handle,
 const Vector3D& position,
 Vector3D& closest,
 Vector3D& normal,
 MsqError& err) const
{
    // find closest node on curve
    size_t closest_sn_id = (size_t) - 1;
    double dist, closest_dist = 1e99; // large number
    for (size_t idx = 0; idx < sn_ids.size(); idx++)
    {
        size_t sn_id = sn_ids[idx];
        const SurfNode &N = surf_domain->surf_nodes[sn_id];
        dist = dist_l2_2(position.to_array(), N.toArray());
        if (dist < closest_dist)
        {
            closest_sn_id = sn_id;
            closest_dist = dist;
        }
    }
    surf_domain->project_to_curve(closest_sn_id, position, closest, normal);
}

void MultiSurfAlign_grid3d::SurfaceCurve::domain_DoF
(const Mesh::EntityHandle* handle_array, unsigned short* dof_array,
 size_t num_handles, Mesquite::MsqError& err) const
{
    surf_domain->domain_DoF(handle_array, dof_array, num_handles, err);
}

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

void MultiSurfAlign_grid3d::SurfacePatch::snap_to
(Mesh::EntityHandle entity_handle, Vector3D &coordinate) const
{
    Mesquite::MsqError err;
    Vector3D pos = coordinate, normal;
    this->closest_point(entity_handle, pos, coordinate, normal, err);
}

void MultiSurfAlign_grid3d::SurfacePatch::normal_at
(Mesh::EntityHandle entity_handle, Vector3D &coordinate) const
{
    Mesquite::MsqError err;
    Vector3D pos = coordinate, closest;
    this->closest_point(entity_handle, pos, closest, coordinate, err);
}

void MultiSurfAlign_grid3d::SurfacePatch::normal_at
(const Mesh::EntityHandle* handles,
 Vector3D coordinates[],
 unsigned count,
 Mesquite::MsqError& err) const
{
    for (unsigned a = 0; a < count; a++)
        this->normal_at(handles[a], coordinates[a]);
}

void MultiSurfAlign_grid3d::SurfacePatch::closest_point
(Mesh::EntityHandle handle,
 const Vector3D& position,
 Vector3D& closest,
 Vector3D& normal,
 MsqError& err) const
{
    // find closest node on patch
    SurfNode CN(-1);
    size_t closest_sn_id = (size_t) - 1;
    double dist, closest_dist = 1e99; // large number
    for (size_t idx = 0; idx < sn_ids.size(); idx++)
    {
        size_t sn_id = sn_ids[idx];
        const SurfNode &N = surf_domain->surf_nodes[sn_id];
        dist = dist_l2(position.to_array(), N.toArray());
        if (dist < closest_dist)
        {
            closest_sn_id = sn_id;
            closest_dist = dist;
            CN = N;
        }
    }

    assert(CN.id >= 0);
    const std::set<size_t> &pids_of_node =
            surf_domain->patch_ids_of_node[closest_sn_id];
    if (pids_of_node.size() != 1)
        surf_domain->__compute_ball(CN, &se_ids, &sf_ids);
    closest = CN.GetProjection(position);
    normal = CN.GetSurfNormal(position);
}

void MultiSurfAlign_grid3d::SurfacePatch::domain_DoF
(const Mesh::EntityHandle* handle_array, unsigned short* dof_array,
 size_t num_handles, Mesquite::MsqError& err) const
{
    surf_domain->domain_DoF(handle_array, dof_array, num_handles, err);
}




